import time
import cv2
from aruco_detector import ArUcoDetector, ESC_KEY
from robot_arm import ETController, ROBOT_IP

if __name__ == "__main__":
    # ArUco detector setup
    detector = ArUcoDetector()
    if not detector.configure_pipeline():
        exit()
    if not detector.start_pipeline():
        exit()

    start_time = time.time()
    frame_count = 0

    try:
        controller = ETController(ROBOT_IP)

        if controller.connect():
            # Get robot state
            success, result, cmd_id = controller.send_cmd("getRobotState")

            # Print result
            if success:
                print("Robot result:", result)
            else:
                print("Failed to get robot state")

            while True:
                color_image = detector.get_frame()
                if color_image is None:
                    continue

                detected_image, detection_result = detector.detect_aruco(color_image)
                cv2.imshow("Color Viewer", detected_image)

                if detection_result.ids is not None:
                    print(f"Detected IDs: {detection_result.ids}")
                    print(f"Euler Angles: {detection_result.euler_angles}")
                    print(f"Translation Vectors: {detection_result.tvecs}")

                frame_count += 1
                elapsed_time = time.time() - start_time
                if elapsed_time > 1.0:
                    fps = frame_count / elapsed_time
                    print(f"FPS: {fps:.2f}")
                    start_time = time.time()
                    frame_count = 0

                key = cv2.waitKey(1)
                if key == ord("q") or key == ESC_KEY:
                    break

    except KeyboardInterrupt:
        pass
    finally:
        controller.disconnect()
        detector.stop_pipeline()
        cv2.destroyAllWindows()
